#include "trackball.h"

using namespace Ogre;
using namespace std;

namespace
{
  const unsigned int doubleClickDelay = 500;
  CEGUI::MouseButton convertOgreButtonToCegui(int buttonID)
  {
    switch (buttonID)
    {
    case MouseEvent::BUTTON0_MASK:
      return CEGUI::LeftButton;
    case MouseEvent::BUTTON1_MASK:
      return CEGUI::RightButton;
    case MouseEvent::BUTTON2_MASK:
      return CEGUI::MiddleButton;
    case MouseEvent::BUTTON3_MASK:
      return CEGUI::X1Button;
    default:
      return CEGUI::LeftButton;
    }
  }
  static const float size = 1;
  /*! Returns "pseudo-distance" from (x,y) to ball of radius size.
   * \arg for a point inside the ball, it is proportional to the euclidean distance to the ball
   * \arg for a point outside the ball, it is proportional to the inverse of this distance (tends to
   * zero) on the ball, the function is continuous.
   */
  float
  projectOnBall(float x, float y)
  {
    const float size2      = size*size;
    const float size_limit = size2*0.5f;

    const float d = x*x + y*y;
    return d < size_limit ? sqrt(size2 - d) : size_limit/sqrt(d);
  }

  /*! Returns a quaternion computed according to the mouse motion. Mouse
   * positions are projected on a deformed ball, centered on (\p cx,\p cy).
   */
  Quaternion deformedBallQuaternion(float qx,float qy,float x, float y, float cx, float cy)
  {
    static const float rotationSensitivity = 1.0f;
    // Points on the deformed ball
    float px = rotationSensitivity * (qx - cx);
    float py = rotationSensitivity * (cy - qy);
    float dx = rotationSensitivity * (x  - cx);
    float dy = rotationSensitivity * (cy -  y);

    const Vector3 p1(px, py, projectOnBall(px, py));
    const Vector3 p2(dx, dy, projectOnBall(dx, dy));
    // Approximation of rotation angle
    // Should be divided by the projectOnBall size, but it is 1.0
    Vector3 axis = p2.crossProduct(p1)/size;;
    const float angle = 2.0 * asin(sqrt(axis.squaredLength() / p1.squaredLength() / p2.squaredLength()));

    axis.normalise();
    return Quaternion(Radian(angle), axis);
  }
  void
  rotate(Camera* c,const Quaternion& q)
  {
    c->setOrientation(c->getOrientation()*q);
  }  
  void
  translate(Camera* c,const Vector3& t)
  {
    c->setPosition(c->getPosition()+t);
  }  
  Vector3
  rotate(const Quaternion& q,const Vector3& v)
  {
    Matrix3 m;
    q.ToRotationMatrix(m);
    return m*v;
  }  
  Vector3
  coordinatesOf(const Quaternion& q,const Vector3& p,const Vector3& src)
  {
    return rotate(q.Inverse(),src - p);
  }
  Vector3
  inverseTransformOf(const Quaternion& q,const Vector3& /*p*/,const Vector3& src)
  {
    return rotate(q,src);
  }
}
//************************************************************
// Implementation of Trackball
//************************************************************
Trackball::Trackball(CEGUI::OgreCEGUIRenderer* r,CEGUI::System* s,Camera* c,SceneManager* m)
  : mGUIRenderer(r),
    mGUISystem(s),
    mCamera(c),
    mSceneManager(m),
    mSceneCenter(Vector3(0.0f,0.0f,0.0f)),
    mSceneRadius(100.0f)
{
  mRevolveAroundPoint = Vector3(0.0f,0.0f,0.0f);
  for (unsigned int i=0;i<3;++i)
  {
    mButtonTimers[i] =  PlatformManager::getSingleton().createTimer();
    mButtonTimers[i]->reset();
    mButtonDown[i] = false;
  }
  mRaySceneQuery = mSceneManager->createRayQuery(Ray());
}
const Vector3&
Trackball::sceneCenter() const
{
  return mSceneCenter;
}
void
Trackball::setSceneCenter(const Vector3& rap)
{
  mSceneCenter = rap;
}
const Real
Trackball::sceneRadius() const
{
  return mSceneRadius;
}
void
Trackball::setSceneRadius(Real r)
{
  mSceneRadius = r;
}
Real
Trackball::distanceRevolveAroundPointToCamera() const
{
  return fabs((mRevolveAroundPoint-mCamera->getPosition()).dotProduct(mCamera->getOrientation().zAxis()));
}
void
Trackball::mousePressed(MouseEvent* e)
{
  if (e->isShiftDown() ||
      e->isControlDown() ||
      e->isMetaDown() ||
      e->isAltDown())
  {
    return;
  }
  CEGUI::System::getSingleton().injectMouseButtonDown(convertOgreButtonToCegui(e->getButtonID()));
  // Determines from pressed buttons the action to take (a click action or a
  // mouse action).
  mMouseAction = NO_MOUSE_ACTION;
  if (e->getButtonID() == InputEvent::BUTTON0_MASK) 
  {
    if (mButtonTimers[0]->getMilliseconds()<doubleClickDelay)
    {
      if (mButtonDown[2])
      {
        // Left double click with right pressed 
        Ray mouseRay = mCamera->getCameraToViewportRay(e->getX(),e->getY());
        mRaySceneQuery->setRay(mouseRay);
        mRaySceneQuery->setSortByDistance(true);
        RaySceneQueryResult &result = mRaySceneQuery->execute();
        RaySceneQueryResult::iterator itr;
        float distance = -1.0f;
        for (itr = result.begin(); itr != result.end(); ++itr)
        {
          if (itr->worldFragment)
          {
            distance = itr->distance;
            break;
          }
          if (itr->movable && itr->movable->getMovableType() == "Entity")
          {
            distance = itr->distance;
          }
        }
        if (distance > 0.0)
        {
          mRevolveAroundPoint = mouseRay.getPoint(distance);
        }
      }
      else if (mButtonDown[1])
      {
        // Left double click with middle pressed
        mRevolveAroundPoint = sceneCenter();
      }
      else
      {
        // Left double click
        align();
      }      
    }
    else
    {
      // Left simple click
      mMouseAction = ROTATE;
      mButtonTimers[0]->reset();  
    }
    mButtonDown[0] = true;
  }    
  else if (e->getButtonID() == InputEvent::BUTTON2_MASK) // Mid Button
  {
    if (mButtonTimers[1]->getMilliseconds()<doubleClickDelay)
    {
      // Middle double click
      showEntireScene();
    }
    else
    {
      // Middle simple click
      mMouseAction = ZOOM;
      mButtonTimers[1]->reset();  
    }
    mButtonDown[1] = true;
  }
  else if (e->getButtonID() == InputEvent::BUTTON1_MASK) 
  {
    if (mButtonTimers[2]->getMilliseconds()<doubleClickDelay)
    {
      if (mButtonDown[0])
      {
        // Right double click with left pressed
      }
      else if (mButtonDown[1])
      {
        // Right double click with middle pressed
      }
      else
      {
        // Right double click
      }      
    }
    else
    {
      // Right simple click
      mMouseAction = TRANSLATE;
      mButtonTimers[2]->reset();  
    }
    mButtonDown[2] = true;
  }  
  e->consume();
  // CEGUI::MouseCursor::getSingleton().hide();
}
void
Trackball::mouseReleased(MouseEvent* e)
{
  CEGUI::System::getSingleton().injectMouseButtonUp(convertOgreButtonToCegui(e->getButtonID()));
  // Restart timers to detect double clicks
  mMouseAction = NO_MOUSE_ACTION;
  if (e->getButtonID() == InputEvent::BUTTON0_MASK) 
  {
    // Left button
    mButtonDown[0] = false;
  }    
  else if (e->getButtonID() == InputEvent::BUTTON2_MASK) 
  {
    // Mid Button 
    mButtonDown[1] = false;
  }
  else if (e->getButtonID() == InputEvent::BUTTON1_MASK) 
  {
    // Right button
    mButtonDown[2] = false;
  }
  mMouseAction = NO_MOUSE_ACTION;
  e->consume();
  //CEGUI::MouseCursor::getSingleton().show();
}
void
Trackball::mouseMoved(MouseEvent* e)
{
  CEGUI::System::getSingleton().injectMouseMove(e->getRelX()*mGUIRenderer->getWidth(), 
                                                e->getRelY()*mGUIRenderer->getHeight());
}
void
Trackball::mouseDragged(MouseEvent* e)
{
  CEGUI::System::getSingleton().injectMouseMove(e->getRelX()*mGUIRenderer->getWidth(), 
                                                e->getRelY()*mGUIRenderer->getHeight());
  if (mMouseAction != NO_MOUSE_ACTION)
  {
    switch (mMouseAction)
    {
    case TRANSLATE:
      {
        Vector3 trans =  Vector3(-e->getRelX()*mCamera->getAspectRatio(),e->getRelY(),0.0f);
        // Scale to fit the screen mouse displacement
        float dst  = distanceRevolveAroundPointToCamera();
        float coef = 2.0f*tan(mCamera->getFOVy().valueRadians()/2.0f)*dst;
        mCamera->moveRelative(coef*trans);

		// Make sure the camera is above the ground before rendering
		if (this->mCamera->getPosition().y < 100.0f)
			this->mCamera->setPosition(this->mCamera->getPosition().x, 100.0f, this->mCamera->getPosition().z);
      }
      break;
    case ZOOM:
      {
        float dst  = distanceRevolveAroundPointToCamera();
        float coef = max(dst,0.2f*sceneRadius());
        mCamera->moveRelative(Vector3(0.0f,0.0f,-coef*e->getRelY()));

		// Make sure the camera is above the ground before rendering
		if (this->mCamera->getPosition().y < 100.0f)
			this->mCamera->setPosition(this->mCamera->getPosition().x, 100.0f, this->mCamera->getPosition().z);
      }
      break;
    case ROTATE:
      {
        float x = e->getX();
        float y = e->getY();
        float qx = e->getX()-e->getRelX();
        float qy = e->getY()-e->getRelY();
        Vector3 trans = mCamera->getProjectionMatrixRS()*(mCamera->getViewMatrix()*mRevolveAroundPoint);
        float cx = 0.5f*(trans.x+1.0f);
        float cy = 0.5f*(trans.y+1.0f); 
        Quaternion rot = deformedBallQuaternion(qx,qy,x,y,cx,cy);       
        Vector3 axis;
        Radian angle;
        rot.ToAngleAxis(angle,axis);
        {
          Quaternion q_ = mCamera->getOrientation();
          Vector3 t_ = mCamera->getPosition();
                  
          q_ = q_*rot;
          q_.normalise(); // Prevents numerical drift
          Vector3 trans = mRevolveAroundPoint+rotate(Quaternion(angle,rotate(q_,axis)),t_-mRevolveAroundPoint)-t_;
          t_ += trans;

		  // Make sure the camera is above the ground before rendering
		  if (t_[1]<100.0f)
			  t_[1]=100.0f;
                  
          mCamera->setOrientation(q_);
          mCamera->setPosition(t_);
        }
      }
      break;
    default:
      break;
    }
    e->consume();
  }
}
void
Trackball::align()
{
  return;
  static const float threshold = 0.85f;
  Vector3 directions[2][3];
  for (int d=0; d<3; ++d)
  {
    Vector3 dir((d==0)? 1.0 : 0.0, (d==1)? 1.0 : 0.0, (d==2)? 1.0 : 0.0);
    directions[0][d] = dir;
    directions[1][d] = inverseTransformOf(mCamera->getOrientation(),mCamera->getPosition(),dir);
  }

  float maxProj = 0.0f;
  float proj;
  unsigned short index[2];
  index[0] = index[1] = 0;
  for (int i=0; i<3; ++i)
    for (int j=0; j<3; ++j)
      if ( (proj=fabs(directions[0][i].dotProduct(directions[1][j]))) >= maxProj )
      {
        index[0] = i;
        index[1] = j;
        maxProj  = proj;
      }

  Quaternion oldOrientation = mCamera->getOrientation();
  Vector3    oldPosition    = mCamera->getPosition();

  float coef = directions[0][index[0]].dotProduct(directions[1][index[1]]);
  if (fabs(coef) >= threshold)
  {
    const Vector3 axis = directions[0][index[0]].crossProduct(directions[1][index[1]]);
    float angle = asin(axis.length());
    if (coef >= 0.0)
      angle = -angle;
    rotate(mCamera,mCamera->getOrientation().Inverse()*Quaternion(Radian(angle),axis)*mCamera->getOrientation());

    // Try to align an other axis direction
    unsigned short d = (index[1]+1) % 3;
    Vector3 dir((d==0)? 1.0 : 0.0, (d==1)? 1.0 : 0.0, (d==2)? 1.0 : 0.0);
    dir = rotate(mCamera->getOrientation(),dir);

    float max = 0.0f;
    for (int i=0; i<3; ++i)
    {
      float proj = fabs(directions[0][i].dotProduct(dir));
      if (proj > max)
      {
        index[0] = i;
        max = proj;
      }
    }

    if (max >= threshold)
    {
      const Vector3 axis = directions[0][index[0]].crossProduct(dir);
      float angle = asin(axis.length());
      if (directions[0][index[0]].dotProduct(dir) >= 0.0)
        angle = -angle;
      rotate(mCamera,mCamera->getOrientation().Inverse()*Quaternion(Radian(angle),axis)*mCamera->getOrientation());
    }
  }
  Vector3 center = Vector3(0.0f,0.0f,0.0f);
  translate(mCamera,center-rotate(mCamera->getOrientation(),coordinatesOf(oldOrientation,oldPosition,center))-mCamera->getPosition());
}
void
Trackball::showEntireScene()
{
  float distance = 0.0f;
  float fovy = mCamera->getFOVy().valueRadians();
  const float yview = mSceneRadius / sinf(fovy/2.0);
  const float xview = mSceneRadius / sinf(atanf(tanf(fovy/2.0) * mCamera->getAspectRatio()));
  distance = max(xview,yview);

  mCamera->setPosition(Vector3(mSceneCenter-distance*mCamera->getDirection()));
}  
// Broken functions that we must however derive because they
// are pure virtuals.
void
Trackball::mouseClicked(MouseEvent*)
{
}
void
Trackball::mouseEntered(MouseEvent*)
{
}
void
Trackball::mouseExited(MouseEvent*)
{
}
